#
#    Copyright (C) 2016 Francisco Javier Parra <franparpe@openmailbox.org>
#
#    This program is free software; you can redistribute it and/or modify
#    it under the terms of the GNU General Public License as published by
#    the Free Software Foundation; either version 2 of the License, or
#    (at your option) any later version.
#
#    This program is distributed in the hope that it will be useful,
#    but WITHOUT ANY WARRANTY; without even the implied warranty of
#    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
#    GNU General Public License for more details.
#
#    You should have received a copy of the GNU General Public License
#    along with this program; if not, write to the Free Software
#    Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301 USA
#

import numpy
from qtcam.core.parameters import SectionParameters
import math

class ModifiedTrapezoidalSection(SectionParameters):
    """The acceleration on this sections take the form fo a trapezoid with
    sinoidal expresions that are located at the extremes instead of the
    descending lines of the trapezoid. This section is used if one wants
    the cam to not have discontinuity when combined with stopped sections.
    This mathematical expresion also allows to keep the acceleration
    more in controll

    
    """
    def __init__(self, LowestPosition=0.0,
                 HighestPosition=50.0, StartAngle=0.0,
                 EndAngle=100.0, Precision=0.1, AngularVelocity=1):
        super(ModifiedTrapezoidalSection, self).__init__(LowestPosition,
                                                         HighestPosition,
                                                         StartAngle,
                                                         EndAngle,
                                                         Precision,
                                                         AngularVelocity)
        
    def ArrayPosition(self):
        """Returns an array with the corresponding position
        
        """
        beta = self.EndAngle - self.StartAngle
        theta_array = self.abcissaeValues - self.StartAngle
        result = numpy.array([])

        for theta in theta_array:
            if 0 <= (theta / beta) < (1/8):
                s = self.LowestPosition + (self.HighestPosition *
                                           (0.38898448 * (theta / beta) -
                                            0.0309544 * numpy.sin((4 *
                                                                   numpy.pi *
                                                                   theta) /
                                                                  beta)))
                result = numpy.append(result, s)
            if (1/8) <= (theta / beta) < (3/8):
                s = self.LowestPosition + (self.HighestPosition *
                                           (2.44406184 * numpy.power(theta /
                                                                     beta, 2) -
                                            0.22203097 * (theta / beta) +
                                            0.00723407))
                result = numpy.append(result, s)
            if (3/8) <= (theta / beta) < (5/8):
                s = self.LowestPosition + (self.HighestPosition *
                                           (1.6110154 * (theta / beta) -
                                            0.0309544 *
                                            numpy.sin(((4 * numpy.pi * theta) /
                                                       (beta)) -
                                                      (numpy.pi)) -
                                            0.3055077))
                result = numpy.append(result, s)

            if (5/8) <= (theta / beta) < (7/8):
                s = self.LowestPosition + (self.HighestPosition *
                                           (-2.44406184 *
                                            numpy.power(theta / beta, 2) +
                                            4.6660917 * (theta / beta) -
                                            1.2292648))
                result = numpy.append(result, s)

            if (7/8) <= (theta / beta) < 1:
                s = self.LowestPosition + (self.HighestPosition *
                                           (0.6110154 +
                                            0.38898448 * (theta / beta) +
                                            0.0309544 *
                                            numpy.sin(((4 * numpy.pi * theta) /
                                                       (beta)) - 3 *
                                                      numpy.pi)))
                result = numpy.append(result, s)

        return result

    def ArrayVelocity(self):
        """Returns an array with the corresponding position
        
        """
        beta = self.EndAngle - self.StartAngle
        theta_array = self.abcissaeValues - self.StartAngle
        result = numpy.array([])

        for theta in theta_array:
            if 0 <= (theta / beta) < (1/8):
                v = (self.AngularVelocity * 0.38898448 *
                     (self.HighestPosition / math.radians(beta)) *
                     (1 - numpy.cos((4 *
                                     numpy.pi *
                                     theta) /
                                    beta)))
                result = numpy.append(result, v)
            if (1/8) <= (theta / beta) < (3/8):
                v = (self.AngularVelocity * (self.HighestPosition / math.radians(beta)) *
                     (4.888124 * (theta / beta) -
                      0.22203097))
                result = numpy.append(result, v)
            if (3/8) <= (theta / beta) < (5/8):
                v = (self.AngularVelocity * (self.HighestPosition / math.radians(beta)) *
                     (1.6110154 -
                      0.38898448 *
                      numpy.cos(((4 * numpy.pi * theta) /
                                 (beta)) - numpy.pi)))
                result = numpy.append(result, v)

            if (5/8) <= (theta / beta) < (7/8):
                v = (self.AngularVelocity * (self.HighestPosition / math.radians(beta)) *
                     (-4.888124 *
                      (theta / beta) +
                      4.6660917))
                result = numpy.append(result, v)

            if (7/8) <= (theta / beta) < 1:
                v = (self.AngularVelocity * (self.HighestPosition / math.radians(beta)) *
                     0.38898448 * (1 +
                                   numpy.cos(((4 * numpy.pi * theta) /
                                              (beta)) - 3 *
                                             numpy.pi)))
                result = numpy.append(result, v)

        return result

    def ArrayAcceleration(self):
        """Returns an array with the corresponding position
        
        """
        beta = self.EndAngle - self.StartAngle
        theta_array = self.abcissaeValues - self.StartAngle
        result = numpy.array([])

        for theta in theta_array:
            if 0 <= (theta / beta) < (1/8):
                a = (numpy.power(self.AngularVelocity, 2) * 4.888124 *
                     (self.HighestPosition / numpy.power(math.radians(beta), 2)) *
                     (numpy.sin((4 *
                                 numpy.pi *
                                 theta) /
                                beta)))
                result = numpy.append(result, a)
            if (1/8) <= (theta / beta) < (3/8):
                a = (numpy.power(self.AngularVelocity, 2) *
                     (self.HighestPosition / numpy.power(math.radians(beta), 2)) *
                     4.888124)
                result = numpy.append(result, a)
            if (3/8) <= (theta / beta) < (5/8):
                a = (numpy.power(self.AngularVelocity, 2) *
                     (self.HighestPosition / numpy.power(math.radians(beta), 2)) *
                     4.888124 *
                     numpy.sin(((4 * numpy.pi * theta) /
                                (beta)) - numpy.pi))
                result = numpy.append(result, a)

            if (5/8) <= (theta / beta) < (7/8):
                a = (numpy.power(self.AngularVelocity, 2) *
                     (self.HighestPosition / numpy.power(math.radians(beta), 2)) *
                     (-4.888124))
                result = numpy.append(result, a)

            if (7/8) <= (theta / beta) < 1:
                a = -(numpy.power(self.AngularVelocity, 2) *
                      (self.HighestPosition / numpy.power(math.radians(beta), 2)) *
                      4.888124 * (numpy.sin(((4 * numpy.pi * theta) /
                                             (beta)) - 3 *
                                            numpy.pi)))
                result = numpy.append(result, a)

        return result

    def ArrayOverAcceleration(self):
        """Returns an array with the corresponding position
        
        """
        beta = self.EndAngle - self.StartAngle
        theta_array = self.abcissaeValues - self.StartAngle
        result = numpy.array([])

        for theta in theta_array:
            if 0 <= (theta / beta) < (1/8):
                j = (numpy.power(self.AngularVelocity, 3) * 61.425769 *
                     (self.HighestPosition / numpy.power(math.radians(beta), 3)) *
                     (numpy.cos((4 *
                                 numpy.pi *
                                 theta) /
                                beta)))
                result = numpy.append(result, j)

            if (1/8) <= (theta / beta) < (3/8):
                j = 0.0
                result = numpy.append(result, j)

            if (3/8) <= (theta / beta) < (5/8):
                j = (numpy.power(self.AngularVelocity, 3) *
                     (self.HighestPosition / numpy.power(math.radians(beta), 3)) *
                     61.425769 *
                     numpy.cos(((4 * numpy.pi * theta) /
                                (beta)) - numpy.pi))
                result = numpy.append(result, j)

            if (5/8) <= (theta / beta) < (7/8):
                j = 0.0
                result = numpy.append(result, j)

            if (7/8) <= (theta / beta) < 1:
                j = -(numpy.power(self.AngularVelocity, 3) *
                      (self.HighestPosition / numpy.power(math.radians(beta), 3)) *
                      61.425769 * (numpy.cos(((4 * numpy.pi * theta) /
                                              (beta)) - 3 *
                                             numpy.pi)))
                result = numpy.append(result, j)

        return result
